#include "abz_encoder.h"
// #include "tim.h"

TIM_HandleTypeDef htim3;

// 根据Z相信号判断编码器角度是否有效
uint8_t encoder_angle_valid = 0;
float speed_encoder = 0.0f;  // 编码器速度
float position_encoder = 0.0f;  // 编码器位置

/**
 * @brief ABZ编码器初始化和启动
 * 
 */
void ABZEncoder_Init(void)
{
  TIM3_Init();
  HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL); // 启动编码器
}

/**
 * @brief 读编码器寄存器
 * 
 * @return uint16_t 编码器寄存器的值
 */
uint16_t Read_Encode()
{
  return __HAL_TIM_GET_COUNTER(&htim3);
}

/**
 * @brief 编码器寄存器清零
 * 
 */
void Set_Encoder_Zero()
{
  __HAL_TIM_SET_COUNTER(&htim3, 0);
  encoder_angle_valid = 1;
}

/**
 * @brief 计算位置和速度
 * 
 */
uint16_t last_encoder_value = 0;
float Cal_Encoder_Speed()
{
  // 计算编码器差异
  int encoder_value = Read_Encode();
  int delta_encoder = encoder_value - last_encoder_value;

  // 处理编码器回绕情况
  if (delta_encoder > 2048)
  {
    delta_encoder -= 4096;
  } else if (delta_encoder < -2048)
  {
    delta_encoder += 4096;
  }
  
  // 计算速度，单位是弧度每秒 (rad/s)
  float speed = (float)delta_encoder / 4096.0f * 2.0f * PI * 1000.0f;

  // 计算位置单位是弧度 (rad)
  position_encoder += (float)delta_encoder / 4096.0f * 2.0f * PI;
  last_encoder_value = encoder_value;
  return speed;
}

void TIM3_Init(void)
{
  TIM_Encoder_InitTypeDef sConfig = {0};
  TIM_MasterConfigTypeDef sMasterConfig = {0};
  htim3.Instance = TIM3;
  htim3.Init.Prescaler = 0;
  htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim3.Init.Period = 4095;
  htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  sConfig.EncoderMode = TIM_ENCODERMODE_TI12;
  sConfig.IC1Polarity = TIM_ICPOLARITY_FALLING;
  sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
  sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
  sConfig.IC1Filter = 0;
  sConfig.IC2Polarity = TIM_ICPOLARITY_FALLING;
  sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
  sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
  sConfig.IC2Filter = 0;
  if (HAL_TIM_Encoder_Init(&htim3, &sConfig) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
}

void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef* tim_encoderHandle)
{

  GPIO_InitTypeDef GPIO_InitStruct = {0};
  if(tim_encoderHandle->Instance==TIM3)
  {
    /* TIM3 clock enable */
    __HAL_RCC_TIM3_CLK_ENABLE();

    __HAL_RCC_GPIOC_CLK_ENABLE();
    /**TIM3 GPIO Configuration
    PC6     ------> TIM3_CH1
    PC7     ------> TIM3_CH2
    */
    GPIO_InitStruct.Pin = GPIO_PIN_6|GPIO_PIN_7;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    GPIO_InitStruct.Alternate = GPIO_AF2_TIM3;
    HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
  }
}

void HAL_TIM_Encoder_MspDeInit(TIM_HandleTypeDef* tim_encoderHandle)
{

  if(tim_encoderHandle->Instance==TIM3)
  {
    __HAL_RCC_TIM3_CLK_DISABLE();

    /**TIM3 GPIO Configuration
    PC6     ------> TIM3_CH1
    PC7     ------> TIM3_CH2
    */
    HAL_GPIO_DeInit(GPIOC, GPIO_PIN_6|GPIO_PIN_7);
  }
}
